//
// Created by engineai on 2024/07/03.
//
/*========================= Gamepad Control ==========================*/
/**
 *
 */

#include "Controllers/DesiredStateCommand.h"
/*=========================== Gait Data ===============================*/
/**
 *
 */
template <typename T>
void DesiredStateData<T>::zero()
{
    // Overall desired state
    stateDes = Vec12<T>::Zero();
    stateTrajDes = Eigen::Matrix<T, 12, 10>::Zero();
}

template struct DesiredStateData<double>;
template struct DesiredStateData<float>;

/**
 *
 */
template <typename T>
void DesiredStateCommand<T>::convertToStateCommands()
{
    data.zero();
    Vec2<float> joystickLeft, joystickRight;

    // T height_cmd(0.3);

    // THIS SHOULD be DISABLE Soon
    if (parameters->use_rc)
    {
        if (rcCommand->mode == RC_mode::QP_STAND)
        {                         // Stand
            joystickLeft[0] = 0.; // Y
            joystickLeft[1] = 0.;
            joystickRight[0] = rcCommand->rpy_des[2]; // Yaw
            // height_cmd = rcCommand->height_variation;
        }
        else if (rcCommand->mode == RC_mode::LOCOMOTION)
        {                                               // Walking
            joystickLeft[0] = rcCommand->v_des[0];      // X
            joystickLeft[1] = rcCommand->v_des[1];      // Y
            joystickRight[0] = rcCommand->omega_des[2]; // Yaw
            joystickRight[1] = rcCommand->omega_des[1]; // Pitch
            gait_type = rcCommand->gait_type;
            cmd_vel_bias_x = rcCommand->cmd_vel_bias_x;
            cmd_vel_bias_y = rcCommand->cmd_vel_bias_y;
            pitch_bias = rcCommand->pitch_bias;
            roll_bias = rcCommand->roll_bias;
            bias_save_mode = rcCommand->bias_save_mode;
            bias_save_mode_pre = rcCommand->bias_save_mode_pre;
            euler_ang_calib_mode = rcCommand->imu_euler_angle_calibration_mode;
            linvel_calib_mode = rcCommand->imu_linvel_calibration_mode;
        }
        else if (rcCommand->mode == RC_mode::TWO_LEG_STANCE)
        { // Two Contact Stand
            // joystickLeft[0] = rcCommand->p_des[1]; // Y
            joystickLeft[1] = rcCommand->p_des[0];    // X
            joystickRight[0] = rcCommand->rpy_des[2]; // Yaw
            joystickRight[1] = rcCommand->rpy_des[1]; // Pitch
            joystickLeft[0] = rcCommand->rpy_des[0];  // Roll
        }
        else
        {
            joystickLeft.setZero();
            joystickRight.setZero();
        }
    }
    else
    { // No Remote Controller
        joystickLeft = gamepadCommand->leftStickAnalog;
        joystickRight = gamepadCommand->rightStickAnalog;
        trigger_pressed = gamepadCommand->a;
    }

    leftAnalogStick = leftAnalogStick * (T(1) - filter) + joystickLeft * filter;
    rightAnalogStick = rightAnalogStick * (T(1) - filter) + joystickRight * filter;

    // Warning!!!!
    // Recommend not to use stateDes
    // We are going to remove it soon

    // Desired states from the controller
    data.stateDes(6) = deadband(leftAnalogStick[0], minVelX, maxVelX);           // forward linear velocity
    data.stateDes(7) = deadband(leftAnalogStick[1], minVelY, maxVelY);           // lateral linear velocity
    data.stateDes(8) = 0.0;                                                      // vertical linear velocity
    data.stateDes(0) = dt * data.stateDes(6);                                    // X position
    data.stateDes(1) = dt * data.stateDes(7);                                    // Y position
    data.stateDes(2) = 0.26;                                                     // Z position height
    data.stateDes(9) = 0.0;                                                      // Roll rate
    data.stateDes(10) = 0.0;                                                     // Pitch rate
    data.stateDes(11) = deadband(rightAnalogStick[0], minTurnRate, maxTurnRate); // Yaw turn rate
    data.stateDes(3) = 0.0;                                                      // Roll
    data.stateDes(4) = deadband(rightAnalogStick[1], minPitch, maxPitch);        // Pitch
    data.stateDes(5) = dt * data.stateDes(11);                                   // Yaw
}

template <typename T>
void DesiredStateCommand<T>::setCommandLimits(T minVelX_in, T maxVelX_in,
                                              T minVelY_in, T maxVelY_in, T minTurnRate_in, T maxTurnRate_in)
{
    minVelX = minVelX_in;
    maxVelX = maxVelX_in;
    minVelY = minVelY_in;
    maxVelY = maxVelY_in;
    minTurnRate = minTurnRate_in;
    maxTurnRate = maxTurnRate_in;
}

/**
 *
 */
template <typename T>
void DesiredStateCommand<T>::desiredStateTrajectory(int N, Vec10<T> dtVec)
{
    A = Mat12<T>::Zero();
    A(0, 0) = 1;
    A(1, 1) = 1;
    A(2, 2) = 1;
    A(3, 3) = 1;
    A(4, 4) = 1;
    A(5, 5) = 1;
    A(6, 6) = 1;
    A(7, 7) = 1;
    A(8, 8) = 1;
    A(9, 9) = 1;
    A(10, 10) = 1;
    A(11, 11) = 1;
    data.stateTrajDes.col(0) = data.stateDes;

    for (int k = 1; k < N; k++)
    {
        A(0, 6) = dtVec(k - 1);
        A(1, 7) = dtVec(k - 1);
        A(2, 8) = dtVec(k - 1);
        A(3, 9) = dtVec(k - 1);
        A(4, 10) = dtVec(k - 1);
        A(5, 11) = dtVec(k - 1);
        data.stateTrajDes.col(k) = A * data.stateTrajDes.col(k - 1);
        for (int i = 0; i < 12; i++)
        {
            // std::cout << data.stateTrajDes(i, k) << " ";
        }
        // std::cout << std::endl;
    }
    // std::cout << std::endl;
}

/**
 *
 */
template <typename T>
float DesiredStateCommand<T>::deadband(float command, T minVal, T maxVal)
{
    if (command < deadbandRegion && command > -deadbandRegion)
    {
        return 0.0;
    }
    else
    {
        return (command / (2)) * (maxVal - minVal);
    }
}

/**
 *
 */
template <typename T>
void DesiredStateCommand<T>::printRawInfo()
{
    // Increment printing iteration
    printIter++;

    // Print at requested frequency
    if (printIter == printNum)
    {
        std::cout << "[DESIRED STATE COMMAND] Printing Raw Gamepad Info...\n";
        std::cout << "---------------------------------------------------------\n";
        std::cout << "Button Start: " << gamepadCommand->start
                  << " | Back: " << gamepadCommand->back << "\n";
        std::cout << "Button A: " << gamepadCommand->a
                  << " | B: " << gamepadCommand->b << " | X: " << gamepadCommand->x
                  << " | Y: " << gamepadCommand->y << "\n";
        std::cout << "Left Stick Button: " << gamepadCommand->leftStickButton
                  << " | X: " << gamepadCommand->leftStickAnalog[0]
                  << " | Y: " << gamepadCommand->leftStickAnalog[1] << "\n";
        std::cout << "Right Analog Button: " << gamepadCommand->rightStickButton
                  << " | X: " << gamepadCommand->rightStickAnalog[0]
                  << " | Y: " << gamepadCommand->rightStickAnalog[1] << "\n";
        std::cout << "Left Bumper: " << gamepadCommand->leftBumper
                  << " | Trigger Switch: " << gamepadCommand->leftTriggerButton
                  << " | Trigger Value: " << gamepadCommand->leftTriggerAnalog
                  << "\n";
        std::cout << "Right Bumper: " << gamepadCommand->rightBumper
                  << " | Trigger Switch: " << gamepadCommand->rightTriggerButton
                  << " | Trigger Value: " << gamepadCommand->rightTriggerAnalog
                  << "\n\n";
        std::cout << std::endl;

        // Reset iteration counter
        printIter = 0;
    }
}

/**
 *
 */
template <typename T>
void DesiredStateCommand<T>::printStateCommandInfo()
{
    // Increment printing iteration
    printIter++;

    // Print at requested frequency
    if (printIter == printNum)
    {
        std::cout << "[DESIRED STATE COMMAND] Printing State Command Info...\n";
        std::cout << "---------------------------------------------------------\n";
        std::cout << "Position X: " << data.stateDes(0)
                  << " | Y: " << data.stateDes(1) << " | Z: " << data.stateDes(2)
                  << "\n";
        std::cout << "Orientation Roll: " << data.stateDes(3)
                  << " | Pitch: " << data.stateDes(4)
                  << " | Yaw: " << data.stateDes(5) << "\n";
        std::cout << "Velocity X: " << data.stateDes(6)
                  << " | Y: " << data.stateDes(7) << " | Z: " << data.stateDes(8)
                  << "\n";
        std::cout << "Angular Velocity X: " << data.stateDes(9)
                  << " | Y: " << data.stateDes(10) << " | Z: " << data.stateDes(11)
                  << "\n";
        std::cout << std::endl;
        std::cout << std::endl;

        // Reset iteration counter
        printIter = 0;
    }
}

template class DesiredStateCommand<double>;
template class DesiredStateCommand<float>;
